/**
 * @file : TimeIntegrator.cpp
 * @author: fenghuwuyu
 * @version 1.0
 * @mail: 3148107006@qq.com
 * @date: 2021年04月19日 星期一 16时09分03秒
 * @brief:
 */

#include "TimeIntegrator.h"

template <int order>
AB<order>::AB()
{
	p = order;
	switch (p)
	{
		case 1:
			beta[0] = 1;
			beta[1] = 0;
			break;

		case 2:
			beta[0] = - 1 / 2;
			beta[1] = 3 / 2;
			beta[2] = 0;
			break;

		case 3:
			beta[0] = 5 / 12;
			beta[1] = - 16 / 12;
			beta[2] = 23 / 12;
			beta[3] = 0;
			break;

		case 4:
			beta[0] = - 9 / 24;
			beta[1] = 37 / 24;
			beta[2] = - 59 / 24;
			beta[3] = 55 / 24;
			beta[4] = 0;
			break;

		default:
			break;
			
	}
	s = p;
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
	for(int i=0;i<6;i++)
	{
		U[0][i] = 1;
	}
}

template <int order>
AB<order>::~AB()
{
	beta.clear();
	U.clear();
}

template <int order>
void AB<order>::Set_E(double _E)
{
	E = _E;
}

template <int order>
void AB<order>::Set_k(double _k)
{
	k = _k;
}

template <int order>
void AB<order>::Set_T(double _T)
{
	T = _T;
}

template <int order>
const double AB<order>::Get_beta()
{
	return beta;
}

template <int order>
const double AB<order>::Get_error()
{
	return e;
}

template <int order>
const double AB<order>::Get_convengence()
{
	return r;
}

template <int order>
const double AB<order>::Get_n()
{
	return n;
}

template <int order>
const void AB<order>::Print_U()
{
	int tmp1 = U.size();
	int tmp2 = U[tmp1].size();
	for(int i=0;i<tmp2;i++)
	{
		std::cout << U[tmp1][i] << std::endl;
	}
}

template <int order>
void AB<order>::initial_value(std::vector<double> &_U0)
{
	U[0] = _U0;
}

template <int order>
std::vector<double> AB<order>::func(ProductRHS *rhs, std::vector<double> &_U)
{
	return rhs->RHS(_U);	
}

template <int order>
void AB<order>::Compute()
{
	// Used for computing U1, U2, U3.
	std::vector<double> tmp1;
	std::vector<double> tmp2;
	std::vector<double> tmp3;
	std::vector<double> y1;
	std::vector<double> y2;
	std::vector<double> y3;
	std::vector<double> y4;
	ThreeBody_RHS *tbrhs;

	int t = U[0].size();
	switch (p)
	{
		case 1:
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 		
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1 / (p+1)) * U[n+p-1][i] + k * beta[j] * func(tbrhs, U[n+j])[i];
					}
				}
				n++;
			}			
			break;

		case 2:
			while(n<1)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1 / (p+1)) * U[n+p-1][i] + k * beta[j] * func(tbrhs, U[n+j])[i];
					}
				}
				n++;
			}			
			break;
		case 3:
			while(n<2)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1 / (p+1)) * U[n+p-1][i] + k * beta[j] * func(tbrhs, U[n+j])[i];
					}
				}
				n++;
			}			
			break;
		case 4:
			while(n<3)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			while(k*n <= T)
			{
				//	U[n+p] = U[n+p-1] + k * sum(beta[j] * func(U[n+j])) 
				for(int i=0;i<t;i++)
				{
					U[n+p][i] = 0;
					for(int j=0;j<=s;j++)
					{
						U[n+p][i] += (1 / (p+1)) * U[n+p-1][i] + k * beta[j] * func(tbrhs, U[n+j])[i];
					}
				}
				n++;
			}			
			break;

		default:
			std::cout << "maybe the order is too high" << std::endl;
			break;			
	}
}


template <int order>
AM<order>::AM()
{
	switch (p)
	{
		case 2:
			beta[0] = 1 / 2;
			beta[1] = 1 / 2;
			break;

		case 3:
			beta[0] = - 1 / 12;
			beta[1] = 8 / 12;
			beta[2] = 5 / 12;
			break;

		case 4:
			beta[0] = 1 / 24;
			beta[1] = - 5 / 24;
			beta[2] = 19 / 24;
			beta[3] = 9 / 24;
			break;

		case 5:
			beta[0] = - 19 / 720;
			beta[1] = 106 / 720;
			beta[2] = - 264 / 720;
			beta[3] = 646 / 720;
			beta[4] = 251 / 720;
			break;

		default:
			break;
			
	}
	p = order;
	s = p-1;
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
	for(int i=0;i<6;i++)
	{
		U[0][i] = 1;
	}
}

template <int order>
AM<order>::~AM()
{
	beta.clear();
	U.clear();
}

template <int order>
void AM<order>::Set_E(double _E)
{
	E = _E;
}

template <int order>
void AM<order>::Set_k(double _k)
{
	k = _k;
}

template <int order>
void AM<order>::Set_T(double _T)
{
	T = _T;
}

template <int order>
const double AM<order>::Get_beta()
{
	return beta;
}

template <int order>
const double AM<order>::Get_error()
{
	return e;
}

template <int order>
const double AM<order>::Get_convengence()
{
	return r;
}

template <int order>
const double AM<order>::Get_n()
{
	return n;
}

template <int order>
const void AM<order>::Print_U()
{
	int tmp1 = U.size();
	int tmp2 = U[tmp1].size();
	for(int i=0;i<tmp2;i++)
	{
		std::cout << U[tmp1][i] << std::endl;
	}
}

template <int order>
void AM<order>::initial_value(std::vector<double> &_U0)
{
	U[0] = _U0;
}

template <int order>
std::vector<double> AM<order>::func(ProductRHS *rhs, std::vector<double> &_U)
{
	return rhs->RHS(_U);	
}

template <int order>
std::vector<double> AM<order>::Newton1(std::vector<std::vector<double>> &_U, int n)
{
	ThreeBody_RHS *tbrhs;

	const double mu = 1 / 81.45;
	double t1 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu - 1) * (_U[n+s][0] + mu - 1);
	double t2 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu) * (_U[n+s][0] + mu);
	Eigen::Matrix<double, 6, 6> Jacobi_f; // f'(U[n+s])
	Eigen::Matrix<double, 6, 6> Jacobi_g; // g'(U[n+s])
	Eigen::Matrix<double, 6, 6> I; // I
	Eigen::Matrix<double, 6, 1> U; // _U[n+s]'s matrix 
	Eigen::Matrix<double, 6, 1> U1; // U[n+s-1]'s matrix 
	Eigen::Matrix<double, 6, 1> tf; // used to storage vector f(U[n+j])
	Eigen::Matrix<double, 6, 1> Nu; // new U[n+s], just is U[n+s]_{m} with input U[n+s]_{m-1}, which the function will return
	Eigen::Matrix<double, 6, 1> b; // b
	Eigen::Matrix<double, 6, 1> g; // g
	Eigen::Matrix<double, 6, 1> f; // f
	Eigen::Matrix<double, 6, 1> t; // tmp Matrix
	std::vector<double> F; // vector f
	std::vector<double> NU; // matrix Nu's vector version, which the function will exactly return 
	t << 0,
		 0,
		 0,
		 0,
		 0,
		 0; // intial t

    F = func(tbrhs, _U[n+s]);                   

	for(int i=0;i<6;i++)
	{
		f(i, 0) = F[i];
	} // set f

	F.clear();                                                                                                   
	for(int i=0;i<6;i++)
	{
		U(i, 0) = _U[n+s][i];
	} // intial U

	for(int i=0;i<6;i++)
	{
		U1(i, 0) = _U[n+s-1][i];
	}

	g << 0,
		 0,
		 0,
		 0,
		 0,
		 0; // intial g

	for(int j=0;j<s;j++)
	{
		F = func(tbrhs, U[n+j]);
		for(int i=0;i<6;i++)
		{
			tf(i, 0) = F[i];
		}
		g = (1/s) * U - (1/s) * k * beta[s] * f - (1/s) * U1 - k * beta[j] * tf;
	}

	I << 1, 0, 0, 0, 0, 0,
		 0, 1, 0, 0, 0, 0,
		 0, 0, 1, 0, 0, 0,
		 0, 0, 0, 1, 0, 0,
		 0, 0, 0, 0, 1, 0,
		 0, 0, 0, 0, 0, 1; // get I

	Jacobi_f << 0, 0, 0, 1, 0, 0,
				0, 0, 0, 0, 1, 0,
				0, 0, 0, 0, 0, 1,
				1-mu * pow(t1, -1.5)+3*mu*(_U[n+s][0]+mu-1)*(_U[n+s][0]+mu-1)*pow(t1, -2.5)-(1-mu)*pow(t2, -1.5)+3*(1-mu)*(_U[n+s][0]+mu)*(_U[n+s][0]+mu)*pow(t2, -2.5), 3*mu*(_U[n+s][0]+mu-1)*pow(t1, -2.5)*_U[n+s][1]+3*(1-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 3*mu*(_U[n+s][0]+mu-1)*pow(t1, -2.5)*_U[n+s][2]+3*(1-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 0, 2, 0,
				3*mu*_U[n+s][1]*pow(t1, -2.5)*(_U[n+s][0]+mu-1)+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 1-mu*pow(t1, -1.5)+3*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][1]-(1-mu)*pow(t2, -1.5)+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][1], 3*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][2]+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][2], -2, 0, 0,
				3*mu*_U[n+s][2]*pow(t1, -2.5)*(_U[n+s][0]+mu-1)+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 3*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][1]+3*(1-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][1], -mu*pow(t1, -1.5)+3*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][2]-(1-mu)*pow(t2, -1.5)+3*(1-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][2];
	Jacobi_g = I - k * beta[s] * Jacobi_f;  // set f'(U) and g'(U) and g'(U) is actually A
		
	 b = Jacobi_g * U - g;

	 Nu = Jacobi_g.fullPivLu().solve(b);

	 for(int i=0;i<6;i++)
	{
		NU[i] = Nu(i, 0);
	}
//	return NU;
	
	
}


template <int order>
void AM<order>::Compute()
{
	// Used for computing U1, U2, U3.
	std::vector<double> tmp1;
	std::vector<double> tmp2;
	std::vector<double> tmp3;
	std::vector<double> y1;
	std::vector<double> y2;
	std::vector<double> y3;
	std::vector<double> y4;
	ThreeBody_RHS *tbrhs;

	std::vector<double> U_1; // Used for Newton method.

	int t = U[0].size();
	switch (p)
	{
		case 2:
			while(k*n <= T)
			{
			}			
			break;

		case 3:
			while(n<1)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{

			}			
			break;

		case 4:
			while(n<2)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			while(k*n <= T)
			{

			}			
			break;
		case 5:
			while(n<3)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			while(k*n <= T)
			{

				n++;
			}			
			break;

		default:
			std::cout << "maybe the order is too high" << std::endl;
			break;			
	}
	tbrhs.~ThreeBody_RHS();
}




RK::RK()
{
	for(int i=0;i<6;i++)
	{
		U[0][i] = 1;
	}
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
}

RK::~RK()
{
	y1.clear();
	y2.clear();		
	y3.clear();		
	y4.clear();
	U.clear();
}

void RK::Set_E(double _E)
{
	E = _E;
}

void RK::Set_k(double _k)
{
	k = _k;
}

void RK::Set_T(double _T)
{
	T = _T;
}

const double RK::Get_error()
{
	return e;
}

const double RK::Get_convengence()
{
	return r;
}

const double RK::Get_n()
{
	return n;
}

const void RK::Print_U()
{
	int tmp1 = U.size();
	int tmp2 = U[tmp1].size();
	for(int i=0;i<tmp2;i++)
	{
		std::cout << U[tmp1][i] << std::endl;
	}
}

void RK::initial_value(std::vector<double> &_U0)
{
	U[0] = _U0;
}

std::vector<double> RK::func(ProductRHS *rhs, std::vector<double> &_U)
{
	return rhs->RHS(_U);
}

void RK::Compute()
{
	std::vector<double> tmp1;
	std::vector<double> tmp2;
	std::vector<double> tmp3;
	int t = U[0].size();
	ThreeBody_RHS *tbrhs;
	while(k*n <= T)
	{		
		y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
		for(int i;i<t;i++)
		{
			tmp1[i] = U[n][i] + (k / 2) * y1[i];
		}
		y2 = func(tbrhs, tmp1);
		for(int i;i<t;i++)
		{
			tmp2[i] = U[n][i] + (k / 2) * y2[i];
		}
		y3 = func(tbrhs, tmp2);
		for(int i;i<t;i++)
		{
			tmp3[i] = U[n][i] + k * y3[i]; 
		}
		y4 = func(tbrhs, tmp3);
		for(int i;i<t;i++)
		{
			U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
		}
		n++;
	}
	tmp1.clear();
	tmp2.clear();
	tmp3.clear();
}

template <int order>
BDF<order>::BDF()
{
		switch (p)
	{
		case 1:
			alpha[0] = - 1;
			alpha[1] = 1;
			beta[0] = 0;
			beta[1] = 1;
			break;

		case 2:
			alpha[0] = 1 / 3;
			alpha[1] = - 4 / 3;
			alpha[2] = 1;
			beta[0] = 0;
			beta[1] = 0;
			beta[2] = 2 / 3;
			break;

		case 3:
			alpha[0] = - 2 / 11;
			alpha[1] = 9 / 11;
			alpha[2] = - 18 / 11;
			alpha[3] = 1;
			beta[0] = 0;
			beta[1] = 0;
			beta[2] = 0;
			beta[3] = 6 / 11;
			break;

		case 4:
			alpha[0] = 3 / 25;
			alpha[1] = - 16 / 25;
			alpha[2] = 36 / 25;
			alpha[3] = - 48 / 25;
			alpha[4] = 1;
			beta[0] = 0;
			beta[1] = 0;
			beta[2] = 0;
			beta[3] = 0;
			beta[4] = 12 / 25;
			break;

		default:
			break;
			
	}
	p = order;
	s = p;
	k = 0.1;
	n = 0;
	E = 0.001;
	e = 10000;
	r = 1;
	T = 10;
	for(int i=0;i<6;i++)
	{
		U[0][i] = 1;
	}
}

template <int order>
BDF<order>::~BDF()
{
	beta.clear();
	alpha.clear();
	U.clear();
}

template <int order>
void BDF<order>::Set_E(double _E)
{
	E = _E;
}

template <int order>
void BDF<order>::Set_k(double _k)
{
	k = _k;
}

template <int order>
void BDF<order>::Set_T(double _T)
{
	T = _T;
}

template <int order>
const double BDF<order>::Get_beta()
{
	return beta;
}

template <int order>
const double BDF<order>::Get_alpha()
{
	return alpha;
}

template <int order>
const double BDF<order>::Get_error()
{
	return e;
}

template <int order>
const double BDF<order>::Get_convengence()
{
	return r;
}

template <int order>
const double BDF<order>::Get_n()
{
	return n;
}

template <int order>
const void BDF<order>::Print_U()
{
	int tmp1 = U.size();
	int tmp2 = U[tmp1].size();
	for(int i=0;i<tmp2;i++)
	{
		std::cout << U[tmp1][i] << std::endl;
	}
}

template <int order>
void BDF<order>::initial_value(std::vector<double> &_U0)
{
	U[0] = _U0;
}

template <int order>
std::vector<double> BDF<order>::func(ProductRHS *rhs, std::vector<double> &_U)
{
	return rhs->RHS(_U);	
}

template <int order>
void BDF<order>::Compute()
{
	// Used for computing U1, U2, U3.
	std::vector<double> tmp1;
	std::vector<double> tmp2;
	std::vector<double> tmp3;
	std::vector<double> y1;
	std::vector<double> y2;
	std::vector<double> y3;
	std::vector<double> y4;
	ThreeBody_RHS *tbrhs;

	std::vector<double> U_1;

	int t = U[0].size();
	switch (p)
	{
		case 1:
			while(k*n <= T)
			{
				fill(U[n+s].begin(),U[n+s].end(), 0);
				while(Max(U[n+s], U[n+s-1]) > k)
				{
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					U_1.clear();
				}
				n++;
			}			
			break;

		case 2:
			while(n<1)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				fill(U[n+s].begin(),U[n+s].end(), 0);
				while(Max(U[n+s], U[n+s-1]) > k)
				{
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					U_1.clear();
				}
				n++;
			}			
			break;
		case 3:
			while(n<2)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				fill(U[n+s].begin(),U[n+s].end(), 0);
				while(Max(U[n+s], U[n+s-1]) > k)
				{
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					U_1.clear();
				}
				n++;
			}			
			break;
		case 4:
			while(n<3)
			{		
				y1 = func(tbrhs, U[0]); //tbrhs is a instance of ThreeBody_RHS 
				for(int i;i<t;i++)
				{
					tmp1[i] = U[n][i] + (k / 2) * y1[i];
				}
				y2 = func(tbrhs, tmp1);
				for(int i;i<t;i++)
				{
					tmp2[i] = U[n][i] + (k / 2) * y2[i];
				}
				y3 = func(tbrhs, tmp2);
				for(int i;i<t;i++)
				{
					tmp3[i] = U[n][i] + k * y3[i]; 
				}
				y4 = func(tbrhs, tmp3);
				for(int i;i<t;i++)
				{
					U[n+1][i] = U[n][i] + (k / 6) * (y1[i] + 2 * y2[i] + 2 * y3[i] + y4[i]);
				}
				n++;
			}
			tmp1.clear();
			tmp2.clear();
			tmp3.clear();
			y1.clear();
			y2.clear();
			y3.clear();
			y4.clear();
			n = 0;
			while(k*n <= T)
			{
				fill(U[n+s].begin(),U[n+s].end(), 0);
				while(Max(U[n+s], U[n+s-1]) > k)
				{
					U_1 = Newton2(U, n);
					U[n+s].swap(U_1);
					U_1.clear();
				}
				n++;
			}			
			break;

		default:
			std::cout << "maybe the order is too high" << std::endl;
			break;			
	}

	tbrhs.~ThreeBody_RHS();
	U_1.clear();
}



template <int order>
std::vector<double> BDF<order>::Newton2(std::vector<std::vector<double>> &_U, int n)
{
	ThreeBody_RHS *tbrhs;
	const double mu = 1 / 81.45;
	double t1 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu - 1) * (_U[n+s][0] + mu - 1);
	double t2 = _U[n+s][1] * _U[n+s][1] + _U[n+s][2] * _U[n+s][2] + (_U[n+s][0] + mu) * (_U[n+s][0] + mu);
	Eigen::Matrix<double, 6, 6> Jacobi_f; // f'(U)
	Eigen::Matrix<double, 6, 6> Jacobi_g; // g'(U)
	Eigen::Matrix<double, 6, 6> I; // I
	Eigen::Matrix<double, 6, 1> U; // _U's matrix 
	Eigen::Matrix<double, 6, 1> Nu; // new U, just is U_{m} with input U_{m-1}, which the function will return
	Eigen::Matrix<double, 6, 1> b; // b
	Eigen::Matrix<double, 6, 1> g; // g
	Eigen::Matrix<double, 6, 1> f; // f
	Eigen::Matrix<double, 6, 1> t; // tmp Matrix 
	std::vector<double> F(6); // f vector version 
	std::vector<double> NU(6); // matrix Nu's vector version, which the function will exactly return 
	t << 0,
		 0,
		 0,
		 0,
		 0,
		 0; // intial t

    F = func(tbrhs, _U[n+s]);                   

	for(int i=0;i<6;i++)
	{
		f(i, 0) = F[i];
	} // set f

	for(int i=0;i<s;i++)
	{
		for(int j=0;j<6;j++)
		{
			t(j, 0) += alpha[i] * _U[n+i][j];
		}
	}


	for(int i=0;i<6;i++)
	{
		U(i, 0) = _U[n+s][i];
	} // intial U


	g = k * beta[s - 1] * f - alpha[s - 1] * U - t; 

		I << 1, 0, 0, 0, 0, 0,
		 0, 1, 0, 0, 0, 0,
		 0, 0, 1, 0, 0, 0,
		 0, 0, 0, 1, 0, 0,
		 0, 0, 0, 0, 1, 0,
		 0, 0, 0, 0, 0, 1; // get I

	Jacobi_f << 0, 0, 0, 1, 0, 0,
				0, 0, 0, 0, 1, 0,
				0, 0, 0, 0, 0, 1,
				1-mu * pow(t1, -1.5)+3*mu*(_U[n+s][0]+mu-1)*(_U[n+s][0]+mu-1)*pow(t1, -2.5)-(1-mu)*pow(t2, -1.5)+3*(1-mu)*(_U[n+s][0]+mu)*(_U[n+s][0]+mu)*pow(t2, -2.5), 3*mu*(_U[n+s][0]+mu-1)*pow(t1, -2.5)*_U[n+s][1]+3*(1-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 3*mu*(_U[n+s][0]+mu-1)*pow(t1, -2.5)*_U[n+s][2]+3*(1-mu)*(_U[n+s][0]+mu)*pow(t2, -2.5)*_U[n+s][1], 0, 2, 0,
				3*mu*_U[n+s][1]*pow(t1, -2.5)*(_U[n+s][0]+mu-1)+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 1-mu*pow(t1, -1.5)+3*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][1]-(1-mu)*pow(t2, -1.5)+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][1], 3*mu*_U[n+s][1]*pow(t1, -2.5)*_U[n+s][2]+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*_U[n+s][2], -2, 0, 0,
				3*mu*_U[n+s][2]*pow(t1, -2.5)*(_U[n+s][0]+mu-1)+3*(1-mu)*_U[n+s][1]*pow(t2, -2.5)*(_U[n+s][0]+mu), 3*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][1]+3*(1-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][1], -mu*pow(t1, -1.5)+3*mu*_U[n+s][2]*pow(t1, -2.5)*_U[n+s][2]-(1-mu)*pow(t2, -1.5)+3*(1-mu)*_U[n+s][2]*pow(t2, -2.5)*_U[n+s][2];
	Jacobi_g = k * beta[s - 1] * Jacobi_f - alpha[s - 1] * I;  // set f'(U) and g'(U) and g'(U) is actually A
		
	b = Jacobi_g * U - g;

	Nu = Jacobi_g.fullPivLu().solve(b);
	for(int i=0;i<6;i++)
	{
		NU[i] = Nu(i, 0);
	}
	return NU;
	
	
}


double Max(std::vector<double> &_U1, std::vector<double> &_U2)
{
	int l1 = _U1.size();
	int l2 = _U2.size();
	double m = 0;
	if(l1 != l2)
	{
		std::cout << "Max function has error." << std::endl;
	}
	else
	{
		m = abs(_U1[0] - _U2[0]);
		for(int i=1;i<l1;i++)
		{
			if(abs(_U1[i]-_U2[i]) <= m)
			{
				// DO NOTHING
			}
			else
			{
				m = abs(_U1[i]-_U2[i]);
			}
		}
	}
	return m;
}
